#include "LaserManualCalibrator.h"

#include <fstream> 
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <stdarg.h>

#define ESC 27

int Hmin_cal = 0;
int Hmax_cal = 256;

int Smin_cal = 0;
int Smax_cal = 256;

int Vmin_cal = 0;
int Vmax_cal = 256;

// для хранения каналов HSV
IplImage* hsv = 0;
IplImage* h_plane = 0;
IplImage* s_plane = 0;
IplImage* v_plane = 0;
// для хранения каналов HSV после преобразования
IplImage* h_range = 0;
IplImage* s_range = 0;
IplImage* v_range = 0;

//
// функции-обработчики ползунков
//
void myTrackbarHmin(int pos) {
    Hmin_cal = pos;
    cvInRangeS(h_plane, cvScalar(Hmin_cal), cvScalar(Hmax_cal), h_range);
}

void myTrackbarHmax(int pos) {
    Hmax_cal = pos;
    cvInRangeS(h_plane, cvScalar(Hmin_cal), cvScalar(Hmax_cal), h_range);
}

void myTrackbarSmin(int pos) {
    Smin_cal = pos;
    cvInRangeS(s_plane, cvScalar(Smin_cal), cvScalar(Smax_cal), s_range);
}

void myTrackbarSmax(int pos) {
    Smax_cal = pos;
    cvInRangeS(s_plane, cvScalar(Smin_cal), cvScalar(Smax_cal), s_range);
}

void myTrackbarVmin(int pos) {
    Vmin_cal = pos;
    cvInRangeS(v_plane, cvScalar(Vmin_cal), cvScalar(Vmax_cal), v_range);
}

void myTrackbarVmax(int pos) {
    Vmax_cal = pos;
    cvInRangeS(v_plane, cvScalar(Vmin_cal), cvScalar(Vmax_cal), v_range);
}

void LaserManualCalibrator :: getLaserManual(){
    // для хранения суммарной картинки
    IplImage* hsv_and = 0;
    int HSVmax = 256;

    assert( calibrationImage != 0 );

    // создаём картинки
    hsv = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 3 );
    h_plane = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    s_plane = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    v_plane = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    h_range = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    s_range = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    v_range = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    hsv_and = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 1 );
    IplImage* calibration = cvCreateImage( cvGetSize(calibrationImage), IPL_DEPTH_8U, 3 );
    //  конвертируем в HSV 
    cvCvtColor( calibrationImage, hsv, CV_BGR2HSV ); 
    // разбиваем на отельные каналы
    cvCvtPixToPlane( hsv, h_plane, s_plane, v_plane, 0 );

    //
    // определяем минимальное и максимальное значение
    // у каналов HSV
    double framemin = 0;
    double framemax = 0;

    cvMinMaxLoc(h_plane, &framemin, &framemax);
    printf("[H] %f x %f\n", framemin, framemax );
    Hmin_cal = framemin;
    Hmax_cal = framemax;
    cvMinMaxLoc(s_plane, &framemin, &framemax);
    printf("[S] %f x %f\n", framemin, framemax );
    Smin_cal = framemin;
    Smax_cal = framemax;
    cvMinMaxLoc(v_plane, &framemin, &framemax);
    printf("[V] %f x %f\n", framemin, framemax );
    Vmin_cal = framemin;
    Vmax_cal = framemax;

    // окна для отображения картинки
    cvNamedWindow("original",CV_WINDOW_AUTOSIZE);
    cvNamedWindow("hsv and",CV_WINDOW_AUTOSIZE);
    cvNamedWindow("Calibration",CV_WINDOW_AUTOSIZE);

    cvCreateTrackbar("Hmin", "Calibration", &Hmin_cal, HSVmax, myTrackbarHmin);
    cvCreateTrackbar("Hmax", "Calibration", &Hmax_cal, HSVmax, myTrackbarHmax);
    cvCreateTrackbar("Smin", "Calibration", &Smin_cal, HSVmax, myTrackbarSmin);
    cvCreateTrackbar("Smax", "Calibration", &Smax_cal, HSVmax, myTrackbarSmax);
    cvCreateTrackbar("Vmin", "Calibration", &Vmin_cal, HSVmax, myTrackbarVmin);
    cvCreateTrackbar("Vmax", "Calibration", &Vmax_cal, HSVmax, myTrackbarVmax);

    while(true){

        // показываем картинку
        cvShowImage("original",calibrationImage);

        // складываем 
        cvAnd(h_range, s_range, hsv_and);
        cvAnd(hsv_and, v_range, hsv_and);

        cvShowImage( "hsv and", hsv_and );
        calibration =  makeCalibrationImg();

        cvShowImage( "Calibration", calibration);

        char c = cvWaitKey(33);
        if (c == ESC) { // если нажата ESC - выходим
            break;
        }
    }
    printf("\n[i] Results:\n" );
    printf("[H] %d x %d\n", Hmin_cal, Hmax_cal );
    printf("[S] %d x %d\n", Smin_cal, Smax_cal );
    printf("[V] %d x %d\n", Vmin_cal, Vmax_cal );

    // освобождаем ресурсы
    cvReleaseImage(&hsv);
    cvReleaseImage(&h_plane);
    cvReleaseImage(&s_plane);
    cvReleaseImage(&v_plane);
    cvReleaseImage(&h_range);
    cvReleaseImage(&s_range);
    cvReleaseImage(&v_range);
    cvReleaseImage(&hsv_and);
    cvReleaseImage(&calibration);
    // удаляем окна
    cvDestroyAllWindows();

    Hmin = Hmin_cal;
    Hmax = Hmax_cal;
    Smin = Smin_cal;
    Smax = Smax_cal;
    Vmin = Vmin_cal;
    Vmax = Vmax_cal;
}


IplImage* LaserManualCalibrator :: makeCalibrationImg() {

    // img - Used for getting the arguments 
    IplImage *img;

    // DispImage - the image in which input images are to be copied
    IplImage *DispImage;

    int size;
    int i;
    int m, n;
    int x, y;

    // w - Maximum number of images in a row 
    // h - Maximum number of images in a column 
    int w, h;

    // scale - How much we have to resize the image
    float scale;
    int max;

    w = 3; h = 2;
    size = 200;

    // Create a new 1 channel image
    DispImage = cvCreateImage( cvSize(100 + size*w, 60 + size*h), 8, 1 );

    IplImage* arr [6];
    arr [0] = h_plane;
    arr [1] = s_plane;
    arr [2] = v_plane;
    arr [3] = h_range;
    arr [4] = s_range;
    arr [5] = v_range;

    for (i = 0, m = 20, n = 20; i < 6; i++, m += (20 + size)) {

        // Get the Pointer to the IplImage
        img = arr[i];

        // Find the width and height of the image
        x = img->width;
        y = img->height;

        // Find whether height or width is greater in order to resize the image
        max = (x > y)? x: y;

        // Find the scaling factor to resize the image
        scale = (float) ( (float) max / size );

        // Used to Align the images
        if( i % w == 0 && m!= 20) {
            m = 20;
            n+= 20 + size;
        }

        // Set the image ROI to display the current image
        cvSetImageROI(DispImage, cvRect(m, n, (int)( x/scale ), (int)( y/scale )));

        // Resize the input image and copy the it to the Single Big Image
        cvResize(img, DispImage);

        // Reset the ROI in order to display the next image
        cvResetImageROI(DispImage);
    }

    return DispImage;
}

LaserManualCalibrator::LaserManualCalibrator(IplImage* img){
    calibrationImage = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 3 );
    // копируем изображение
    cvCopyImage(img, calibrationImage);
    getLaserManual();
}

LaserManualCalibrator::~LaserManualCalibrator(){
    cvDestroyAllWindows();
}

void LaserManualCalibrator::getParams(int* Hmini, int* Hmaxi, int* Smini, int* Smaxi, int* Vmini, int* Vmaxi) const{
    *Hmini = Hmin;
    *Hmaxi = Hmax;
    *Smini = Smin;
    *Smaxi = Smax;
    *Vmini = Vmin;
    *Vmaxi = Vmax;
}

